# -*- coding: utf-8 -*-
"""
Created on Tue Apr  5 12:59:47 2022

@author: lizhuang
"""
import sympy
import numpy as np
a,b,c,x,y = sympy.symbols("a b c x y",real=True)
rc1=0.2
l=0.4
l2=1.2
rc2=0.6
a=(rc1*sympy.cos(x)+rc1*sympy.sin(x)*sympy.I)
b=(l*sympy.cos(x)+l*sympy.sin(x)*sympy.I)+(rc2*sympy.cos(y)+sympy.I*rc2*sympy.sin(y))
c=(l*sympy.cos(x)+l*sympy.sin(x)*sympy.I)+(l2*sympy.cos(y)+l2*sympy.sin(y)*sympy.I)
funcs = sympy.Matrix([a,b,c])
args = sympy.Matrix([x,y])
res = funcs.jacobian(args)
print(res)
d,e = sympy.symbols("d e",real=True)
rp2=0.3
d=(l*sympy.cos(x)+l*sympy.sin(x)*sympy.I)+(rp2*sympy.cos(y)+rp2*sympy.sin(y)*sympy.I)
e=(l*sympy.cos(x)+l*sympy.sin(x)*sympy.I)+(l2*sympy.cos(y)+l2*sympy.sin(y)*sympy.I)
funcs1 = sympy.Matrix([d,e])
args1 = sympy.Matrix([x,y])
res1 = funcs1.jacobian(args1)
print(res1)
g=9.8
m1=1.2
m2=3.6
m3=6
d=-(l*sympy.cos(x)/l2*sympy.cos(y))
mg=np.array([[-m1*g*sympy.I,-m2*g*sympy.I,-m3*g*sympy.I]])
print(mg.shape)
dd=mg.dot(res)
print(dd.shape)
#print(dd)
fx2=10
fy2=10
fx3=20
f=np.array([-fx2*sympy.I+fy2,fx3*sympy.I])
mm1=25
m=np.array([mm1,0])
dmn=np.array([[1],[d]])
print(dmn.shape)
重力作用=dd.dot(dmn)
print(重力作用)
print(重力作用.shape)
f作用=f.dot(res1)
f效果=f作用.dot(dmn)
等效力矩=f效果+重力作用+mm1
print(等效力矩)
#计算力矩=等效力矩.subs({x:2,y:2})
